﻿using System;
using System.Collections.Generic;
using System.Text;
using ElteIk.RoboSoccer.Core;
using ElteIk.RoboSoccer.Core.Physics;

namespace ElteIk.RoboSoccer.DavidTactics
{
    class Variables
    {
        public float r1;
        public float r2;
        public float s1;
        public float s2;

        public Variables(float r1, float r2, float s1, float s2)
        {
            this.r1 = r1;
            this.r2 = r2;
            this.s1 = s1;
            this.s2 = s2;
        }
    }

    class MovingAction : IAction
    {
        private const float arrivedTreshold = 10.0f;

        private RobotPlayer robot;
        private Vector2D destination;
        private Vector2D next;
        private bool recording;
        private List<Variables> actions;

        public MovingAction(RobotPlayer robot, Vector2D destination, Vector2D next)
        {
            this.robot = robot;
            this.destination = destination;
            this.next = next;
            recording = false;
            actions = new List<Variables>();
        }

        public void Do()
        {
            HeadFor(destination);
        }

        public bool HasEnded()
        {
            bool ended = (destination - robot.Position).Length < arrivedTreshold;
            /*if (ended)
                TeachDirector();*/
            return ended;
        }

        private void HeadFor(Vector2D destination)
        {
            Vector2D heading = robot.Heading.Normalized;
            Vector2D toDestination = destination - robot.Position;
            Vector2D toNext = next - destination;
            float a1 = heading.X;
            float a2 = heading.Y;

            // Coordinates of toDestination in heading's coordinate system.
            float r1 = -a2 * toDestination.X + a1 * toDestination.Y;
            float r2 = a1 * toDestination.X + a2 * toDestination.Y;

            // Coordinates of toNext in heading's coordinate system.
            float s1 = -a2 * toNext.X + a1 * toNext.Y;
            float s2 = a1 * toNext.X + a2 * toDestination.Y;

            robot.MotorLeft = DavidTactics.Director.LeftEngine(r1, r2, s1, s2);
            robot.MotorRight = DavidTactics.Director.RightEngine(r1, r2, s1, s2);

            if (recording)
                actions.Add(new Variables(r1, r2, s1, s2));
        }

        public void TeachDirector()
        {
            DavidTactics.Director.Learn(actions, 0.1f, 1.0f);
        }

        private void HeadForScripted(Vector2D destination)
        {
            Vector2D heading = robot.Heading.Normalized;
            Vector2D delta = destination - robot.Position;
            float a1 = heading.X;
            float a2 = heading.Y;
            float direction = -a2 * delta.X + a1 * delta.Y;
            if (Math.Abs(direction) < Math.Sin(Math.PI / 12)) // Forward.
            {
                robot.MotorLeft = 0.5f;
                robot.MotorRight = 0.5f;
            }
            else if (direction < 0) // Left.
            {
                robot.MotorLeft = 0.5f;
                robot.MotorRight = 1.0f;
            }
            else // Right.
            {
                robot.MotorLeft = 1.0f;
                robot.MotorRight = 0.5f;
            }
        }
    }
}
